/*
 * A V4L2 driver for imx291 Raw cameras.
 *
 * Copyright (c) 2017 by Allwinnertech Co., Ltd.  http://www.allwinnertech.com
 *
 * Authors:  Zhao Wei <zhaowei@allwinnertech.com>
 *    Yang Feng <yangfeng@allwinnertech.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 */

#include <linux/init.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/i2c.h>
#include <linux/delay.h>
#include <linux/videodev2.h>
#include <linux/clk.h>
#include <media/v4l2-device.h>
#include <media/v4l2-mediabus.h>
#include <linux/io.h>

#include "camera.h"
#include "sensor_helper.h"

MODULE_AUTHOR("lwj");
MODULE_DESCRIPTION("A low-level driver for IMX291 sensors");
MODULE_LICENSE("GPL");

#define MCLK              37125000
#define VREF_POL          V4L2_MBUS_VSYNC_ACTIVE_HIGH
#define HREF_POL          V4L2_MBUS_HSYNC_ACTIVE_HIGH
#define CLK_POL           V4L2_MBUS_PCLK_SAMPLE_FALLING
#define V4L2_IDENT_SENSOR  0x0291

#define VMAX 1125	/*1136 for 24M 1125 for 37.125Mhz*/

/*#define DOL_ENABLE*/
#define DOL_RHS1      55	/*133*/
#define DOL_RATIO     16

/*
 * Our nominal (default) frame rate.
 */

#define SENSOR_FRAME_RATE 30

/*
 * The IMX291 i2c address
 */
#define I2C_ADDR 0x34

#define SENSOR_NUM 0x2
#define SENSOR_NAME "imx291_dvp"
#define SENSOR_NAME_2 "imx291_dvp_2"

/*
 * The default register settings
 *
 */

static struct regval_list sensor_default_regs[] = {

};

static struct regval_list sensor_1080p30_regs[] = {
	{0x3003, 0x01},
	{REG_DLY, 0x10},
	{0x3000, 0x01},
	{0x3002, 0x00},
	{0x3005, 0x01},
	{0x3007, 0x00},
	{0x3009, 0x02},
	{0x300a, 0xf0},
	{0x300f, 0x00},
	{0x3010, 0x21},
	{0x3012, 0x64},
	{0x3016, 0x09},
	{0x3018, 0x70},
	{0x3019, 0x04},
	{0x301c, 0x30},
	{0x301d, 0x11},
	{0x3046, 0x01},
	{0x304a, 0xc2},
	{0x304b, 0x0a},
	{0x305c, 0x18},
	{0x305d, 0x00},
	{0x305e, 0x20},
	{0x305f, 0x01},
	{0x3070, 0x02},
	{0x3071, 0x11},
	{0x309b, 0x10},
	{0x309c, 0x22},
	{0x30a2, 0x02},
	{0x30a6, 0x20},
	{0x30a8, 0x20},
	{0x30aa, 0x20},
	{0x30ac, 0x20},
	{0x30b0, 0x43},
	{0x3119, 0x9e},
	{0x311c, 0x1e},
	{0x311e, 0x08},
	{0x3128, 0x05},
	{0x3129, 0x00},
	{0x313d, 0x83},
	{0x3150, 0x03},
	{0x315e, 0x1a},
	{0x3164, 0x1a},
	{0x317c, 0x00},
	{0x317e, 0x00},
	{0x31ec, 0x0e},
	{0x32b8, 0x50},
	{0x32b9, 0x10},
	{0x32ba, 0x00},
	{0x32bb, 0x04},
	{0x32c8, 0x50},
	{0x32c9, 0x10},
	{0x32ca, 0x00},
	{0x32cb, 0x04},
	{0x332c, 0xd3},
	{0x332d, 0x10},
	{0x332e, 0x0d},
	{0x3358, 0x06},
	{0x3359, 0xe1},
	{0x335a, 0x11},
	{0x3360, 0x1e},
	{0x3361, 0x61},
	{0x3362, 0x10},
	{0x33b0, 0x50},
	{0x33b2, 0x1a},
	{0x33b3, 0x04},
	{0x3405, 0x20},
	{0x3407, 0x03},
	{0x3414, 0x0a},
	{0x3418, 0x49},
	{0x3419, 0x04},
	{0x3441, 0x0c},
	{0x3442, 0x0c},
	{0x3443, 0x03},
	{0x3444, 0x20},
	{0x3445, 0x25},
	{0x3446, 0x47},
	{0x3447, 0x00},
	{0x3448, 0x1f},
	{0x3449, 0x00},
	{0x344a, 0x17},
	{0x344b, 0x00},
	{0x344c, 0x0f},
	{0x344d, 0x00},
	{0x344e, 0x17},
	{0x344f, 0x00},
	{0x3450, 0x47},
	{0x3451, 0x00},
	{0x3452, 0x0f},
	{0x3453, 0x00},
	{0x3454, 0x0f},
	{0x3455, 0x00},
	{0x3472, 0x9c},
	{0x3473, 0x07},
	{0x3480, 0x49},

	{0x3000, 0x00},
};

static struct regval_list sensor_720p_regs[] = {
	{0x3003, 0x01},
	{REG_DLY, 0x10},
	{0x3000, 0x01},
	{0x3002, 0x00},
	{0x3005, 0x01},
	{0x3007, 0x10},	/*720p*/
	{0x3009, 0x02},	/*30fs*/
	{0x300a, 0xf0},
	{0x300f, 0x00},
	{0x3010, 0x21},
	{0x3012, 0x64},
	{0x3013, 0x00},
	{0x3016, 0x09},
	{0x3018, 0xEE},
	{0x3019, 0x02},
	{0x301c, 0xC8},
	{0x301d, 0x19},
	{0x3020, 0x01},
	{0x3021, 0x00},
	{0x3046, 0x01},
	{0x304A, 0xC2},
	{0x304b, 0x0a},
	{0x305c, 0x20},
	{0x305d, 0x00},
	{0x305e, 0x20},
	{0x305f, 0x01},
	{0x3070, 0x02},
	{0x3071, 0x11},
	{0x309b, 0x10},
	{0x309c, 0x22},
	{0x30a2, 0x02},
	{0x30a6, 0x20},
	{0x30a8, 0x20},
	{0x30aa, 0x20},
	{0x30ac, 0x20},
	{0x30b0, 0x43},
	{0x3119, 0x9e},
	{0x311c, 0x1e},
	{0x311e, 0x08},
	{0x3128, 0x05},
	{0x3129, 0x00},
	{0x313d, 0x83},
	{0x3150, 0x03},
	{0x315e, 0x1a},
	{0x3164, 0x1a},
	{0x317c, 0x00},
	{0x317e, 0x00},
	{0x31ec, 0x0e},
	{0x32b8, 0x50},
	{0x32b9, 0x10},
	{0x32ba, 0x00},
	{0x32bb, 0x04},
	{0x32c8, 0x50},
	{0x32c9, 0x10},
	{0x32ca, 0x00},
	{0x32cb, 0x04},
	{0x332c, 0xd3},
	{0x332d, 0x10},
	{0x332e, 0x0d},
	{0x3358, 0x06},
	{0x3359, 0xe1},
	{0x335a, 0x11},
	{0x3360, 0x1e},
	{0x3361, 0x61},
	{0x3362, 0x10},
	{0x33b0, 0x50},
	{0x33b2, 0x1a},
	{0x33b3, 0x04},
	{0x3405, 0x20},
	{0x3407, 0x03},
	{0x3414, 0x0a},
	{0x3418, 0x49},
	{0x3419, 0x04},
	{0x3441, 0x0c},
	{0x3442, 0x0c},
	{0x3443, 0x03},
	{0x3444, 0x20},
	{0x3445, 0x25},
	{0x3446, 0x47},
	{0x3447, 0x00},
	{0x3448, 0x1f},
	{0x3449, 0x00},
	{0x344a, 0x17},
	{0x344b, 0x00},
	{0x344c, 0x0f},
	{0x344d, 0x00},
	{0x344e, 0x17},
	{0x344f, 0x00},
	{0x3450, 0x47},
	{0x3451, 0x00},
	{0x3452, 0x0f},
	{0x3453, 0x00},
	{0x3454, 0x0f},
	{0x3455, 0x00},
	{0x3472, 0x9c},
	{0x3473, 0x07},
	{0x3480, 0x49},

	{0x3000, 0x00},
};

/*
 * Here we'll try to encapsulate the changes for just the output
 * video format.
 *
 */

static struct regval_list sensor_fmt_raw[] = {
};

/*
 * Code for dealing with controls.
 * fill with different sensor module
 * different sensor module has different settings here
 * if not support the follow function ,retrun -EINVAL
 */

static int sensor_g_exp(struct v4l2_subdev *sd, __s32 *value)
{
	struct sensor_info *info = to_state(sd);

	*value = info->exp;
	sensor_dbg("sensor_get_exposure = %d\n", info->exp);
	return 0;
}

static int imx291_sensor_vts;
static int sensor_s_exp(struct v4l2_subdev *sd, unsigned int exp_val)
{
	data_type explow, expmid, exphigh;
	int exptime;
#ifdef DOL_ENABLE
	int exp_val_m;
#endif
#ifdef DOL_3FRAME
	int exp_val_s;
#endif
	struct sensor_info *info = to_state(sd);

	if (exp_val > 0x1fffff)
		exp_val = 0x1fffff;

	exptime = imx291_sensor_vts - (exp_val >> 4) - 1;

#ifdef DOL_ENABLE
	exptime = (imx291_sensor_vts << 1) - (exp_val >> 4) - 1;
	if (exptime < DOL_RHS1 + 2) {
		exptime = DOL_RHS1 + 2;
		exp_val = ((imx291_sensor_vts << 1) - exptime - 1) << 4;
	}

	exphigh = (unsigned char)((0x0030000 & exptime) >> 16);
	expmid = (unsigned char)((0x000ff00 & exptime) >> 8);
	explow = (unsigned char)((0x00000ff & exptime));

	sensor_write(sd, 0x3024, explow);
	sensor_write(sd, 0x3025, expmid);
	sensor_write(sd, 0x3026, exphigh);

	exp_val_m = (exp_val + DOL_RATIO / 2) / DOL_RATIO;
	if (exp_val_m < 16)
		exp_val_m = 16;
	exptime = DOL_RHS1 - (exp_val_m >> 4) - 1;
	if (exptime < 2)
		exptime = 2;

	sensor_dbg("long exp_val: %d, short exp_val: %d, exptime: %d\n",
		   exp_val, exp_val_m, exptime);
	sensor_dbg("0x3024: 0x%2x, 0x3025: 0x%2x, ", explow, expmid);

	exphigh = (unsigned char)((0x0030000 & exptime) >> 16);
	expmid = (unsigned char)((0x000ff00 & exptime) >> 8);
	explow = (unsigned char)((0x00000ff & exptime));

	sensor_write(sd, 0x3020, explow);
	sensor_write(sd, 0x3021, expmid);
	sensor_write(sd, 0x3022, exphigh);
	sensor_dbg("0x3020: 0x%2x, 0x3021: 0x%2x\n", explow, expmid);
#else
	exphigh = (unsigned char)((0x0030000 & exptime) >> 16);
	expmid = (unsigned char)((0x000ff00 & exptime) >> 8);
	explow = (unsigned char)((0x00000ff & exptime));

	sensor_write(sd, 0x3020, explow);
	sensor_write(sd, 0x3021, expmid);
	sensor_write(sd, 0x3022, exphigh);
#endif

#ifdef DOL_3FRAME
	exp_val_s = exp_val >> 4;
	exptime = imx291_sensor_vts - (exp_val_s >> 4) - 1;

	exphigh = (unsigned char)((0x0030000 & exptime) >> 16);
	expmid = (unsigned char)((0x000ff00 & exptime) >> 8);
	explow = (unsigned char)((0x00000ff & exptime));

	sensor_write(sd, 0x3028, explow);
	sensor_write(sd, 0x3029, expmid);
	sensor_write(sd, 0x302a, exphigh);
#endif

	info->exp = exp_val;
	return 0;
}

static int sensor_g_gain(struct v4l2_subdev *sd, __s32 *value)
{
	struct sensor_info *info = to_state(sd);
	*value = info->gain;
	sensor_dbg("sensor_get_gain = %d\n", info->gain);
	return 0;
}

unsigned char gain2db[4081] = {
	0,   2,   3,	 5,   6,   8,	9,  11,  12,  13,  14,	15,  16,  17,
	18,  19,  20,	21,  22,  23,  23,  24,  25,  26,  27,	27,  28,  29,
	29,  30,  31,	31,  32,  32,  33,  34,  34,  35,  35,	36,  36,  37,
	37,  38,  38,	39,  39,  40,  40,  41,  41,  41,  42,	42,  43,  43,
	44,  44,  44,	45,  45,  45,  46,  46,  47,  47,  47,	48,  48,  48,
	49,  49,  49,	50,  50,  50,  51,  51,  51,  52,  52,	52,  52,  53,
	53,  53,  54,	54,  54,  54,  55,  55,  55,  56,  56,	56,  56,  57,
	57,  57,  57,	58,  58,  58,  58,  59,  59,  59,  59,	60,  60,  60,
	60,  60,  61,	61,  61,  61,  62,  62,  62,  62,  62,	63,  63,  63,
	63,  63,  64,	64,  64,  64,  64,  65,  65,  65,  65,	65,  66,  66,
	66,  66,  66,	66,  67,  67,  67,  67,  67,  68,  68,	68,  68,  68,
	68,  69,  69,	69,  69,  69,  69,  70,  70,  70,  70,	70,  70,  71,
	71,  71,  71,	71,  71,  71,  72,  72,  72,  72,  72,	72,  73,  73,
	73,  73,  73,	73,  73,  74,  74,  74,  74,  74,  74,	74,  75,  75,
	75,  75,  75,	75,  75,  75,  76,  76,  76,  76,  76,	76,  76,  77,
	77,  77,  77,	77,  77,  77,  77,  78,  78,  78,  78,	78,  78,  78,
	78,  79,  79,	79,  79,  79,  79,  79,  79,  79,  80,	80,  80,  80,
	80,  80,  80,	80,  80,  81,  81,  81,  81,  81,  81,	81,  81,  81,
	82,  82,  82,	82,  82,  82,  82,  82,  82,  83,  83,	83,  83,  83,
	83,  83,  83,	83,  83,  84,  84,  84,  84,  84,  84,	84,  84,  84,
	84,  85,  85,	85,  85,  85,  85,  85,  85,  85,  85,	86,  86,  86,
	86,  86,  86,	86,  86,  86,  86,  86,  87,  87,  87,	87,  87,  87,
	87,  87,  87,	87,  87,  88,  88,  88,  88,  88,  88,	88,  88,  88,
	88,  88,  88,	89,  89,  89,  89,  89,  89,  89,  89,	89,  89,  89,
	89,  90,  90,	90,  90,  90,  90,  90,  90,  90,  90,	90,  90,  91,
	91,  91,  91,	91,  91,  91,  91,  91,  91,  91,  91,	91,  92,  92,
	92,  92,  92,	92,  92,  92,  92,  92,  92,  92,  92,	93,  93,  93,
	93,  93,  93,	93,  93,  93,  93,  93,  93,  93,  93,	94,  94,  94,
	94,  94,  94,	94,  94,  94,  94,  94,  94,  94,  94,	95,  95,  95,
	95,  95,  95,	95,  95,  95,  95,  95,  95,  95,  95,	95,  96,  96,
	96,  96,  96,	96,  96,  96,  96,  96,  96,  96,  96,	96,  96,  97,
	97,  97,  97,	97,  97,  97,  97,  97,  97,  97,  97,	97,  97,  97,
	97,  98,  98,	98,  98,  98,  98,  98,  98,  98,  98,	98,  98,  98,
	98,  98,  98,	99,  99,  99,  99,  99,  99,  99,  99,	99,  99,  99,
	99,  99,  99,	99,  99,  99, 100, 100, 100, 100, 100, 100, 100, 100,
	100, 100, 100, 100, 100, 100, 100, 100, 100, 101, 101, 101, 101, 101,
	101, 101, 101, 101, 101, 101, 101, 101, 101, 101, 101, 101, 101, 102,
	102, 102, 102, 102, 102, 102, 102, 102, 102, 102, 102, 102, 102, 102,
	102, 102, 102, 102, 103, 103, 103, 103, 103, 103, 103, 103, 103, 103,
	103, 103, 103, 103, 103, 103, 103, 103, 103, 104, 104, 104, 104, 104,
	104, 104, 104, 104, 104, 104, 104, 104, 104, 104, 104, 104, 104, 104,
	104, 104, 105, 105, 105, 105, 105, 105, 105, 105, 105, 105, 105, 105,
	105, 105, 105, 105, 105, 105, 105, 105, 106, 106, 106, 106, 106, 106,
	106, 106, 106, 106, 106, 106, 106, 106, 106, 106, 106, 106, 106, 106,
	106, 106, 107, 107, 107, 107, 107, 107, 107, 107, 107, 107, 107, 107,
	107, 107, 107, 107, 107, 107, 107, 107, 107, 107, 108, 108, 108, 108,
	108, 108, 108, 108, 108, 108, 108, 108, 108, 108, 108, 108, 108, 108,
	108, 108, 108, 108, 108, 109, 109, 109, 109, 109, 109, 109, 109, 109,
	109, 109, 109, 109, 109, 109, 109, 109, 109, 109, 109, 109, 109, 109,
	109, 110, 110, 110, 110, 110, 110, 110, 110, 110, 110, 110, 110, 110,
	110, 110, 110, 110, 110, 110, 110, 110, 110, 110, 110, 110, 111, 111,
	111, 111, 111, 111, 111, 111, 111, 111, 111, 111, 111, 111, 111, 111,
	111, 111, 111, 111, 111, 111, 111, 111, 111, 112, 112, 112, 112, 112,
	112, 112, 112, 112, 112, 112, 112, 112, 112, 112, 112, 112, 112, 112,
	112, 112, 112, 112, 112, 112, 112, 112, 113, 113, 113, 113, 113, 113,
	113, 113, 113, 113, 113, 113, 113, 113, 113, 113, 113, 113, 113, 113,
	113, 113, 113, 113, 113, 113, 113, 114, 114, 114, 114, 114, 114, 114,
	114, 114, 114, 114, 114, 114, 114, 114, 114, 114, 114, 114, 114, 114,
	114, 114, 114, 114, 114, 114, 114, 115, 115, 115, 115, 115, 115, 115,
	115, 115, 115, 115, 115, 115, 115, 115, 115, 115, 115, 115, 115, 115,
	115, 115, 115, 115, 115, 115, 115, 115, 115, 116, 116, 116, 116, 116,
	116, 116, 116, 116, 116, 116, 116, 116, 116, 116, 116, 116, 116, 116,
	116, 116, 116, 116, 116, 116, 116, 116, 116, 116, 116, 117, 117, 117,
	117, 117, 117, 117, 117, 117, 117, 117, 117, 117, 117, 117, 117, 117,
	117, 117, 117, 117, 117, 117, 117, 117, 117, 117, 117, 117, 117, 117,
	117, 118, 118, 118, 118, 118, 118, 118, 118, 118, 118, 118, 118, 118,
	118, 118, 118, 118, 118, 118, 118, 118, 118, 118, 118, 118, 118, 118,
	118, 118, 118, 118, 118, 119, 119, 119, 119, 119, 119, 119, 119, 119,
	119, 119, 119, 119, 119, 119, 119, 119, 119, 119, 119, 119, 119, 119,
	119, 119, 119, 119, 119, 119, 119, 119, 119, 119, 119, 120, 120, 120,
	120, 120, 120, 120, 120, 120, 120, 120, 120, 120, 120, 120, 120, 120,
	120, 120, 120, 120, 120, 120, 120, 120, 120, 120, 120, 120, 120, 120,
	120, 120, 120, 120, 121, 121, 121, 121, 121, 121, 121, 121, 121, 121,
	121, 121, 121, 121, 121, 121, 121, 121, 121, 121, 121, 121, 121, 121,
	121, 121, 121, 121, 121, 121, 121, 121, 121, 121, 121, 121, 122, 122,
	122, 122, 122, 122, 122, 122, 122, 122, 122, 122, 122, 122, 122, 122,
	122, 122, 122, 122, 122, 122, 122, 122, 122, 122, 122, 122, 122, 122,
	122, 122, 122, 122, 122, 122, 122, 123, 123, 123, 123, 123, 123, 123,
	123, 123, 123, 123, 123, 123, 123, 123, 123, 123, 123, 123, 123, 123,
	123, 123, 123, 123, 123, 123, 123, 123, 123, 123, 123, 123, 123, 123,
	123, 123, 123, 123, 124, 124, 124, 124, 124, 124, 124, 124, 124, 124,
	124, 124, 124, 124, 124, 124, 124, 124, 124, 124, 124, 124, 124, 124,
	124, 124, 124, 124, 124, 124, 124, 124, 124, 124, 124, 124, 124, 124,
	124, 124, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125,
	125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125,
	125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125, 125,
	125, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126,
	126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126,
	126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126,
	126, 126, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127,
	127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127,
	127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127,
	127, 127, 127, 127, 127, 128, 128, 128, 128, 128, 128, 128, 128, 128,
	128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128,
	128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128,
	128, 128, 128, 128, 128, 128, 128, 128, 128, 129, 129, 129, 129, 129,
	129, 129, 129, 129, 129, 129, 129, 129, 129, 129, 129, 129, 129, 129,
	129, 129, 129, 129, 129, 129, 129, 129, 129, 129, 129, 129, 129, 129,
	129, 129, 129, 129, 129, 129, 129, 129, 129, 129, 129, 129, 129, 129,
	130, 130, 130, 130, 130, 130, 130, 130, 130, 130, 130, 130, 130, 130,
	130, 130, 130, 130, 130, 130, 130, 130, 130, 130, 130, 130, 130, 130,
	130, 130, 130, 130, 130, 130, 130, 130, 130, 130, 130, 130, 130, 130,
	130, 130, 130, 130, 130, 130, 130, 131, 131, 131, 131, 131, 131, 131,
	131, 131, 131, 131, 131, 131, 131, 131, 131, 131, 131, 131, 131, 131,
	131, 131, 131, 131, 131, 131, 131, 131, 131, 131, 131, 131, 131, 131,
	131, 131, 131, 131, 131, 131, 131, 131, 131, 131, 131, 131, 131, 131,
	131, 131, 132, 132, 132, 132, 132, 132, 132, 132, 132, 132, 132, 132,
	132, 132, 132, 132, 132, 132, 132, 132, 132, 132, 132, 132, 132, 132,
	132, 132, 132, 132, 132, 132, 132, 132, 132, 132, 132, 132, 132, 132,
	132, 132, 132, 132, 132, 132, 132, 132, 132, 132, 132, 132, 132, 133,
	133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 133,
	133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 133,
	133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 133,
	133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 133, 134, 134,
	134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 134,
	134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 134,
	134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 134,
	134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 134, 135, 135,
	135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135,
	135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135,
	135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135,
	135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135, 135,
	135, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136,
	136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136,
	136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136,
	136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136, 136,
	136, 136, 136, 136, 136, 137, 137, 137, 137, 137, 137, 137, 137, 137,
	137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 137,
	137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 137,
	137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 137,
	137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 137, 138, 138,
	138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138,
	138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138,
	138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138,
	138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138, 138,
	138, 138, 138, 138, 138, 138, 138, 139, 139, 139, 139, 139, 139, 139,
	139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139,
	139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139,
	139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139,
	139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139, 139,
	139, 139, 139, 139, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140,
	140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140,
	140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140,
	140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140,
	140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140, 140,
	140, 140, 140, 140, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141,
	141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141,
	141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141,
	141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141,
	141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141, 141,
	141, 141, 141, 141, 141, 141, 142, 142, 142, 142, 142, 142, 142, 142,
	142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142,
	142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142,
	142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142,
	142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 142,
	142, 142, 142, 142, 142, 142, 142, 142, 142, 142, 143, 143, 143, 143,
	143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143,
	143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143,
	143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143,
	143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143,
	143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143, 143,
	143, 143, 143, 143, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144,
	144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144,
	144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144,
	144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144,
	144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144,
	144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 144, 145,
	145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145,
	145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145,
	145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145,
	145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145,
	145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145,
	145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 145, 146, 146,
	146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146,
	146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146,
	146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146,
	146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146,
	146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146,
	146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146, 146,
	147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147,
	147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147,
	147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147,
	147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147,
	147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147,
	147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147, 147,
	147, 147, 147, 147, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148,
	148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148,
	148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148,
	148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148,
	148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148,
	148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148,
	148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 148, 149, 149,
	149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149,
	149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149,
	149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149,
	149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149,
	149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149,
	149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149, 149,
	149, 149, 149, 149, 149, 149, 149, 149, 149, 150, 150, 150, 150, 150,
	150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150,
	150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150,
	150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150,
	150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150,
	150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150,
	150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150,
	150, 150, 150, 150, 150, 150, 150, 150, 150, 151, 151, 151, 151, 151,
	151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151,
	151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151,
	151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151,
	151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151,
	151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151,
	151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151,
	151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 151, 152,
	152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152,
	152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152,
	152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152,
	152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152,
	152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152,
	152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152,
	152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152, 152,
	152, 152, 152, 152, 152, 152, 153, 153, 153, 153, 153, 153, 153, 153,
	153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153,
	153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153,
	153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153,
	153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153,
	153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153,
	153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153,
	153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153, 153,
	153, 153, 153, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154,
	154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154,
	154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154,
	154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154,
	154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154,
	154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154,
	154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154,
	154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154, 154,
	154, 154, 154, 154, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155,
	155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155,
	155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155,
	155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155,
	155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155,
	155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155,
	155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155,
	155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155, 155,
	155, 155, 155, 155, 155, 155, 155, 155, 155, 156, 156, 156, 156, 156,
	156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156,
	156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156,
	156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156,
	156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156,
	156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156,
	156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156,
	156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156,
	156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156, 156,
	156, 156, 156, 156, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157,
	157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157,
	157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157,
	157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157,
	157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157,
	157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157,
	157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157,
	157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157,
	157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157, 157,
	157, 157, 157, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158,
	158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158,
	158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158,
	158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158,
	158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158,
	158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158,
	158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158,
	158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158,
	158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158, 158,
	158, 158, 158, 158, 158, 158, 158, 159, 159, 159, 159, 159, 159, 159,
	159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159,
	159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159,
	159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159,
	159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159,
	159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159,
	159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159,
	159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159,
	159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159,
	159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159, 159,
	159, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160,
	160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160,
	160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160,
	160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160,
	160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160,
	160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160,
	160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160,
	160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160,
	160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160,
	160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160, 160,
	161, 161, 161, 161, 161, 161, 161,
};
char gain_mode_buf = 0x02;
unsigned int count;
static int sensor_s_gain(struct v4l2_subdev *sd, int gain_val)
{
	struct sensor_info *info = to_state(sd);
	int ret;
	data_type rdval;
	char gain_mode = 0x02;

	ret = sensor_read(sd, 0x3009, &rdval);
	if (ret != 0)
		return ret;
	if (gain_val < 1 * 16)
		gain_val = 16;

	if (gain_val < 16 * 16)	{
		gain_mode = rdval & 0xef;
		sensor_write(sd, 0x3014, gain2db[gain_val - 16]);
	} else if (gain_val < 512 * 16) {
		gain_mode = rdval | 0x10;
		sensor_write(sd, 0x3014, gain2db[gain_val / 2 - 16]);
	} else if (gain_val < 1024 * 16) {
		gain_mode = rdval | 0x10;
		sensor_write(sd, 0x3014, gain2db[gain_val / 4 - 16] + 20);
	} else if (gain_val < 2048 * 16) {
		gain_mode = rdval | 0x10;
		sensor_write(sd, 0x3014, gain2db[gain_val / 8 - 16] + 40);
	} else {
		gain_mode = rdval | 0x10;
		sensor_write(sd, 0x3014, gain2db[gain_val / 16 - 16] + 60);
	}

	if (0 != (count++))
		sensor_write(sd, 0x3009, gain_mode_buf);
	gain_mode_buf = gain_mode;
	info->gain = gain_val;

	return 0;
}

static int sensor_s_exp_gain(struct v4l2_subdev *sd,
			     struct sensor_exp_gain *exp_gain)
{
	struct sensor_info *info = to_state(sd);
	int exp_val, gain_val;

	exp_val = exp_gain->exp_val;
	gain_val = exp_gain->gain_val;

	if (gain_val < 1 * 16)
		gain_val = 16;

	if (exp_val > 0xfffff)
		exp_val = 0xfffff;

	sensor_s_exp(sd, exp_val);
	sensor_s_gain(sd, gain_val);

	sensor_dbg("imx291 sensor_set_gain exp_val= %d, gain_val = %d Done!\n",
	       exp_val, gain_val);

	info->exp = exp_val;
	info->gain = gain_val;
	return 0;
}

static int sensor_s_sw_stby(struct v4l2_subdev *sd, int on_off)
{
	int ret = 0;
	return ret;
}

/*
 * Stuff that knows about the sensor.
 */
static int sensor_power(struct v4l2_subdev *sd, int on)
{
	int ret = 0;

	switch (on) {
	case STBY_ON:
		sensor_dbg("STBY_ON!\n");
		cci_lock(sd);
		ret = sensor_s_sw_stby(sd, STBY_ON);
		if (ret < 0)
			sensor_err("soft stby falied!\n");
		usleep_range(10000, 12000);
		cci_unlock(sd);
		break;
	case STBY_OFF:
		sensor_dbg("STBY_OFF!\n");
		cci_lock(sd);
		usleep_range(10000, 12000);
		ret = sensor_s_sw_stby(sd, STBY_OFF);
		if (ret < 0)
			sensor_err("soft stby off falied!\n");
		cci_unlock(sd);
		break;
	case PWR_ON:
		sensor_print("PWR_ON!\n");
		cci_lock(sd);
		vin_gpio_set_status(sd, PWDN, 1);
		vin_gpio_set_status(sd, RESET, 1);
		vin_gpio_set_status(sd, POWER_EN, 1);
		vin_gpio_write(sd, RESET, CSI_GPIO_LOW);
		vin_gpio_write(sd, PWDN, CSI_GPIO_LOW);
		vin_gpio_write(sd, POWER_EN, CSI_GPIO_HIGH);
		vin_set_pmu_channel(sd, IOVDD, ON);
		vin_set_pmu_channel(sd, AVDD, ON);
		vin_set_pmu_channel(sd, DVDD, ON);
		usleep_range(7000, 8000);
		vin_set_mclk_freq(sd, MCLK);
		vin_set_mclk(sd, ON);
		usleep_range(10000, 12000);
		vin_gpio_write(sd, RESET, CSI_GPIO_HIGH);
		vin_gpio_write(sd, PWDN, CSI_GPIO_HIGH);
		usleep_range(10000, 12000);
		cci_unlock(sd);
		break;
	case PWR_OFF:
		sensor_print("PWR_OFF!\n");
		cci_lock(sd);
		vin_gpio_set_status(sd, PWDN, 1);
		vin_gpio_set_status(sd, RESET, 1);
		vin_gpio_write(sd, RESET, CSI_GPIO_LOW);
		vin_gpio_write(sd, PWDN, CSI_GPIO_LOW);
		vin_set_mclk(sd, OFF);
		vin_set_pmu_channel(sd, AFVDD, OFF);
		vin_set_pmu_channel(sd, AVDD, OFF);
		vin_set_pmu_channel(sd, IOVDD, OFF);
		vin_set_pmu_channel(sd, DVDD, OFF);
		vin_gpio_write(sd, POWER_EN, CSI_GPIO_LOW);
		vin_gpio_set_status(sd, RESET, 0);
		vin_gpio_set_status(sd, PWDN, 0);
		vin_gpio_set_status(sd, POWER_EN, 0);
		cci_unlock(sd);
		break;
	default:
		return -EINVAL;
	}

	return 0;
}

static int sensor_reset(struct v4l2_subdev *sd, u32 val)
{
	switch (val) {
	case 0:
		vin_gpio_write(sd, RESET, CSI_GPIO_HIGH);
		usleep_range(10000, 12000);
		break;
	case 1:
		vin_gpio_write(sd, RESET, CSI_GPIO_LOW);
		usleep_range(10000, 12000);
		break;
	default:
		return -EINVAL;
	}
	return 0;
}

static int sensor_detect(struct v4l2_subdev *sd)
{
	data_type rdval = 0;

	sensor_read(sd, 0x3008, &rdval);
	sensor_print("%s read value is 0x%x\n", __func__, rdval);
	return 0;
}

static int sensor_init(struct v4l2_subdev *sd, u32 val)
{
	int ret;
	struct sensor_info *info = to_state(sd);

	sensor_dbg("sensor_init\n");

	/*Make sure it is a target sensor */
	ret = sensor_detect(sd);
	if (ret) {
		sensor_err("chip found is not an target chip.\n");
		return ret;
	}

	info->focus_status = 0;
	info->low_speed = 0;
	info->width = HD1080_WIDTH;
	info->height = HD1080_HEIGHT;
	info->hflip = 0;
	info->vflip = 0;
	info->gain = 0;

	info->tpf.numerator = 1;
	info->tpf.denominator = 30;	/* 30fps */

	info->preview_first_flag = 1;

	return 0;
}

static long sensor_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
{
	int ret = 0;
	struct sensor_info *info = to_state(sd);

	switch (cmd) {
	case GET_CURRENT_WIN_CFG:
		if (info->current_wins != NULL) {
			memcpy(arg, info->current_wins,
			       sizeof(struct sensor_win_size));
			ret = 0;
		} else {
			sensor_err("empty wins!\n");
			ret = -1;
		}
		break;
	case SET_FPS:
		ret = 0;
		break;
	case VIDIOC_VIN_SENSOR_EXP_GAIN:
		ret = sensor_s_exp_gain(sd, (struct sensor_exp_gain *)arg);
		break;
	case VIDIOC_VIN_SENSOR_CFG_REQ:
		sensor_cfg_req(sd, (struct sensor_config *)arg);
		break;
	default:
		return -EINVAL;
	}
	return ret;
}

/*
 * Store information about the video data format.
 */
static struct sensor_format_struct sensor_formats[] = {
	{
	 .desc = "Raw RGB Bayer",
	 .mbus_code = MEDIA_BUS_FMT_SGBRG12_1X12,
	 .regs = sensor_fmt_raw,
	 .regs_size = ARRAY_SIZE(sensor_fmt_raw),
	 .bpp = 1
	 },
};

#define N_FMTS ARRAY_SIZE(sensor_formats)

/*
 * Then there is the issue of window sizes.  Try to capture the info here.
 */

static struct sensor_win_size sensor_win_sizes[] = {
	/* 1080P */
	{
	 .width = HD1080_WIDTH,
	 .height = HD1080_HEIGHT,
	 .hoffset = 0,
	 .voffset = 0,
	 .hts = 4400,
	 .vts = VMAX,
	 .pclk = 150 * 1000 * 1000,
	 .mipi_bps = 223 * 1000 * 1000,
	 .fps_fixed = 30,
	 .bin_factor = 1,
	 .intg_min = 1 << 4,
	 .intg_max = (VMAX - 2) << 4,
	 .gain_min = 1 << 4,
	 .gain_max = 4096 << 4,
	 .regs = sensor_1080p30_regs,
	 .regs_size = ARRAY_SIZE(sensor_1080p30_regs),
	 .set_size = NULL,
	 },

	/* 720P */
	{
	 .width = HD720_WIDTH,
	 .height = HD720_HEIGHT,
	 .hoffset = 0,
	 .voffset = 6,
	 .hts = 6600,
	 .vts = 750,
	 .pclk = 150 * 1000 * 1000,
	 .mipi_bps = 223 * 1000 * 1000,
	 .fps_fixed = 30,
	 .bin_factor = 1,
	 .intg_min = 1 << 4,
	 .intg_max = (750 - 2) << 4,
	 .gain_min = 1 << 4,
	 .gain_max = 4096 << 4,
	 .regs = sensor_720p_regs,
	 .regs_size = ARRAY_SIZE(sensor_720p_regs),
	 .set_size = NULL,
	 },
};

#define N_WIN_SIZES (ARRAY_SIZE(sensor_win_sizes))

static int sensor_g_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
				struct v4l2_mbus_config *cfg)
{
	cfg->type = V4L2_MBUS_PARALLEL;
	cfg->flags = V4L2_MBUS_MASTER | VREF_POL | HREF_POL | CLK_POL;

	return 0;
}

static int sensor_g_ctrl(struct v4l2_ctrl *ctrl)
{
	struct sensor_info *info =
			container_of(ctrl->handler, struct sensor_info, handler);
	struct v4l2_subdev *sd = &info->sd;

	switch (ctrl->id) {
	case V4L2_CID_GAIN:
		return sensor_g_gain(sd, &ctrl->val);
	case V4L2_CID_EXPOSURE:
		return sensor_g_exp(sd, &ctrl->val);
	}
	return -EINVAL;
}

static int sensor_s_ctrl(struct v4l2_ctrl *ctrl)
{
	struct sensor_info *info =
			container_of(ctrl->handler, struct sensor_info, handler);
	struct v4l2_subdev *sd = &info->sd;

	switch (ctrl->id) {
	case V4L2_CID_GAIN:
		return sensor_s_gain(sd, ctrl->val);
	case V4L2_CID_EXPOSURE:
		return sensor_s_exp(sd, ctrl->val);
	}
	return -EINVAL;
}

static int sensor_reg_init(struct sensor_info *info)
{
	int ret;
	struct v4l2_subdev *sd = &info->sd;
	struct sensor_format_struct *sensor_fmt = info->fmt;
	struct sensor_win_size *wsize = info->current_wins;
	struct sensor_exp_gain exp_gain;

	ret = sensor_write_array(sd, sensor_default_regs,
				 ARRAY_SIZE(sensor_default_regs));
	if (ret < 0) {
		sensor_err("write sensor_default_regs error\n");
		return ret;
	}

	sensor_dbg("sensor_reg_init\n");

	sensor_write_array(sd, sensor_fmt->regs, sensor_fmt->regs_size);

	if (wsize->regs)
		sensor_write_array(sd, wsize->regs, wsize->regs_size);

	if (wsize->set_size)
		wsize->set_size(sd);

	info->width = wsize->width;
	info->height = wsize->height;
	imx291_sensor_vts = wsize->vts;

	exp_gain.exp_val = 12480;
	exp_gain.gain_val = 48;
	sensor_s_exp_gain(sd, &exp_gain);

	sensor_print("s_fmt set width = %d, height = %d\n", wsize->width,
		     wsize->height);

	return 0;
}

static int sensor_s_stream(struct v4l2_subdev *sd, int enable)
{
	struct sensor_info *info = to_state(sd);

	sensor_print("%s on = %d, %d*%d %x\n", __func__, enable,
		     info->current_wins->width,
		     info->current_wins->height, info->fmt->mbus_code);

	if (!enable)
		return 0;

	return sensor_reg_init(info);
}

/* ----------------------------------------------------------------------- */

static const struct v4l2_ctrl_ops sensor_ctrl_ops = {
	.g_volatile_ctrl = sensor_g_ctrl,
	.s_ctrl = sensor_s_ctrl,
};

static const struct v4l2_subdev_core_ops sensor_core_ops = {
	.reset = sensor_reset,
	.init = sensor_init,
	.s_power = sensor_power,
	.ioctl = sensor_ioctl,
#ifdef CONFIG_COMPAT
	.compat_ioctl32 = sensor_compat_ioctl32,
#endif
};

static const struct v4l2_subdev_video_ops sensor_video_ops = {
	.s_stream = sensor_s_stream,
};

static const struct v4l2_subdev_pad_ops sensor_pad_ops = {
	.enum_mbus_code = sensor_enum_mbus_code,
	.enum_frame_size = sensor_enum_frame_size,
	.get_fmt = sensor_get_fmt,
	.set_fmt = sensor_set_fmt,
	.get_mbus_config = sensor_g_mbus_config,
};

static const struct v4l2_subdev_ops sensor_ops = {
	.core = &sensor_core_ops,
	.video = &sensor_video_ops,
	.pad = &sensor_pad_ops,
};

/* ----------------------------------------------------------------------- */
static struct cci_driver cci_drv[] = {
	{
		.name = SENSOR_NAME,
		.addr_width = CCI_BITS_16,
		.data_width = CCI_BITS_8,
	}, {
		.name = SENSOR_NAME_2,
		.addr_width = CCI_BITS_16,
		.data_width = CCI_BITS_8,
	}
};

static int sensor_init_controls(struct v4l2_subdev *sd, const struct v4l2_ctrl_ops *ops)
{
	struct sensor_info *info = to_state(sd);
	struct v4l2_ctrl_handler *handler = &info->handler;
	struct v4l2_ctrl *ctrl;
	int ret = 0;

	v4l2_ctrl_handler_init(handler, 2);

	v4l2_ctrl_new_std(handler, ops, V4L2_CID_GAIN, 1 * 1600,
			      256 * 1600, 1, 1 * 1600);
	ctrl = v4l2_ctrl_new_std(handler, ops, V4L2_CID_EXPOSURE, 0,
			      65536 * 16, 1, 0);
	if (ctrl != NULL)
		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;

	if (handler->error) {
		ret = handler->error;
		v4l2_ctrl_handler_free(handler);
	}

	sd->ctrl_handler = handler;

	return ret;
}

static int sensor_dev_id;

static int sensor_probe(struct i2c_client *client,
			const struct i2c_device_id *id)
{
	struct v4l2_subdev *sd;
	struct sensor_info *info;
	int i;

	info = kzalloc(sizeof(struct sensor_info), GFP_KERNEL);
	if (info == NULL)
		return -ENOMEM;
	sd = &info->sd;

	if (client) {
		for (i = 0; i < SENSOR_NUM; i++) {
			if (!strcmp(cci_drv[i].name, client->name))
				break;
		}
		cci_dev_probe_helper(sd, client, &sensor_ops, &cci_drv[i]);
	} else {
		cci_dev_probe_helper(sd, client, &sensor_ops, &cci_drv[sensor_dev_id++]);
	}

	sensor_init_controls(sd, &sensor_ctrl_ops);

	mutex_init(&info->lock);
#ifdef CONFIG_SAME_I2C
	info->sensor_i2c_addr = I2C_ADDR >> 1;
#endif
	info->fmt = &sensor_formats[0];
	info->fmt_pt = &sensor_formats[0];
	info->win_pt = &sensor_win_sizes[0];
	info->fmt_num = N_FMTS;
	info->win_size_num = N_WIN_SIZES;
	info->sensor_field = V4L2_FIELD_NONE;
	info->af_first_flag = 1;
	info->exp = 0;
	info->gain = 0;

	return 0;
}
static int sensor_remove(struct i2c_client *client)
{
	struct v4l2_subdev *sd;
	int i;

	if (client) {
		for (i = 0; i < SENSOR_NUM; i++) {
			if (!strcmp(cci_drv[i].name, client->name))
				break;
		}
		sd = cci_dev_remove_helper(client, &cci_drv[i]);
	} else {
		sd = cci_dev_remove_helper(client, &cci_drv[sensor_dev_id++]);
	}
	kfree(to_state(sd));
	return 0;
}

static const struct i2c_device_id sensor_id[] = {
	{SENSOR_NAME, 0},
	{}
};

static const struct i2c_device_id sensor_id_2[] = {
	{SENSOR_NAME_2, 0},
	{}
};

MODULE_DEVICE_TABLE(i2c, sensor_id);
MODULE_DEVICE_TABLE(i2c, sensor_id_2);

static struct i2c_driver sensor_driver[] = {
	{
		.driver = {
			   .owner = THIS_MODULE,
			   .name = SENSOR_NAME,
			   },
		.probe = sensor_probe,
		.remove = sensor_remove,
		.id_table = sensor_id,
	}, {
		.driver = {
			   .owner = THIS_MODULE,
			   .name = SENSOR_NAME_2,
			   },
		.probe = sensor_probe,
		.remove = sensor_remove,
		.id_table = sensor_id_2,
	},
};
static __init int init_sensor(void)
{
	int i, ret = 0;

	sensor_dev_id = 0;

	for (i = 0; i < SENSOR_NUM; i++)
		ret = cci_dev_init_helper(&sensor_driver[i]);

	return ret;
}

static __exit void exit_sensor(void)
{
	int i;

	sensor_dev_id = 0;

	for (i = 0; i < SENSOR_NUM; i++)
		cci_dev_exit_helper(&sensor_driver[i]);
}

module_init(init_sensor);
module_exit(exit_sensor);
